/******************************************************************
Copyright © LibKz 2017~2023. All rights reserved.
* @projectName   LibKz
* @author        Raiiwhen
* @email         masterraii@icloud.com
* @origin        \06_Car\01_TJCat\01_TJ_Cat_WheelSpeedLoop\ALGO
*******************************************************************/
#include "PID.h"

PID paw_vel[4];
PID paw_pos[4];
PID paw_mom[4];

void PID_Init(PID* obj, float kP, float kI, float kD, float deadzone, u16 dt){
	obj->kP = kP;
	obj->kI = kI;
	obj->kD = kD;
	obj->dt = dt;
	obj->deadzone = deadzone;
}

/*
expect: rpm of wheel
real: rpm that feed back
return: cmd corrence that normalized to 1
*/
float PID_Update(PID* obj, float expect, float real){
	float err = expect - real;
	
	//if(err < obj->deadzone)err=0;;
	
	obj->P = obj->kP * err;
	obj->I += obj->kI * err;
	obj->D = obj->kD * (err - obj->last_err);
	
	obj->last_err = err;
	obj->real = real;
	obj->exp = expect;
	
	return obj->P + obj->I + obj->D;
}

void PID_Reset(PID* obj){
	obj->I = 0;
	obj->last_err = 0;
}

